import rclpy
import cv2
import cv_bridge
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from origincar_msg.msg import Sign
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32
from ai_msgs.msg import PerceptionTargets
from pyzbar import pyzbar
from time import time, sleep

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")

        self.bridge = cv_bridge.CvBridge()

        self.image_sub = self.create_subscription(CompressedImage, '/image/compressed', self.image_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.qr_pub = self.create_publisher(Sign, '/sign_switch', 10)
        
        self.sign_sub = self.create_subscription(Int32, 'sign_foxglove', self.sign_callback, 10)

        self.twist = Twist()

        self.is_following_line = True
        self.is_avoiding_obstacle = False
        self.external_control = False

        self.obstacle_sub = self.create_subscription(
            PerceptionTargets,
            'hobot_dnn_detection',
            self.obstacle_callback,
            10
        )

        self.confidence_threshold = 0.87
        self.y_axis_threshold = 300
        self.last_twist_msg = Twist()
        self.obstacle_center_x = None
        self.obstacle_direction = None

        self.avoid_angular_z = 0.0
        self.avoid_start_time = None
        self.avoid_turn_end_time = None
        self.drive_straight_end_time = None
        self.resume_direction_end_time = None

        self.need_speed_reduction = True  # 新增的状态标志，默认为需要减速

        # PID控制器参数
        self.kp = 0.95          # 比例增益       0.00158333
        self.ki = 0.00          # 积分增益       
        self.kd = 0.8           # 微分增益       0.0012

        self.prev_error = 0.0
        self.integral = 0.0

        self.create_timer(0.1, self.update)  # 添加一个定时器来定期调用update函数

    def sign_callback(self, msg):
        self.get_logger().info(f"Received sign: {msg.data}")
        if msg.data == 5:
            self.is_following_line = False
            self.external_control = True
            self.is_avoiding_obstacle = False
            self.get_logger().info("Deactivating line following due to external control.")
        elif msg.data == 6:
            self.is_following_line = True
            self.external_control = False
            self.get_logger().info("Activating line following.")

    def image_callback(self, msg):
        if not self.is_following_line or self.is_avoiding_obstacle:
            return

        self.get_logger().info("Image callback triggered.")
        try:
            np_arr = np.frombuffer(msg.data, np.uint8)
            image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"CvBridge Error: {e}")
            return

        decoded_objects = pyzbar.decode(image)
        for obj in decoded_objects:
            data = obj.data.decode('utf-8')
            self.get_logger().info(f"QR Code detected: {data}")
            qr_msg = Sign()
            self.twist.linear.x = 0.0
            self.twist.angular.z = 0.0
            self.cmd_vel_pub.publish(self.twist)
            if data == 'ClockWise':
                qr_msg.sign_data = 3
            elif data == 'AntiClockWise':
                qr_msg.sign_data = 4
            self.get_logger().info(f'Publishing: {qr_msg.sign_data}')
            self.qr_pub.publish(qr_msg)
            return

        height, width, _ = image.shape
        roi_height = int(height / 3)         # 定义感兴趣区域的高度，即图像高度的1/3
        roi_bottom = height                  # 感兴趣区域的底部位置，即图像的最底部
        roi_top = roi_bottom - roi_height    # 感兴趣区域的顶部位置
        roi = image[roi_top:roi_bottom, :]   # 从图像中提取出这个感兴趣区域（从底部到顶部的1/3区域）

        # 高斯滤波
        blurred = cv2.GaussianBlur(roi, (5, 5), 0)

        # 转换为HSV颜色空间
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
        
        # 根据阈值去除背景并进行二值化处理
        lower_black = np.array([0, 0, 0])
        upper_black = np.array([180, 255, 50])
        mask = cv2.inRange(hsv, lower_black, upper_black)

        # 腐蚀与膨胀操作
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=1)
        mask = cv2.dilate(mask, kernel, iterations=1)

        # 多点检测中心点
        M1 = cv2.moments(mask[:roi_height//3, :])
        M2 = cv2.moments(mask[roi_height//3:2*roi_height//3, :])
        M3 = cv2.moments(mask[2*roi_height//3:, :])

        cx1, cx2, cx3 = None, None, None

        if M1['m00'] > 0:
            cx1 = int(M1['m10'] / M1['m00'])
        if M2['m00'] > 0:
            cx2 = int(M2['m10'] / M2['m00'])
        if M3['m00'] > 0:
            cx3 = int(M3['m10'] / M3['m00'])

        cx_values = [cx for cx in [cx1, cx2, cx3] if cx is not None]

        if cx_values:
            cx = int(np.mean(cx_values))
            if M3['m00'] > 0:
                cy = int(M3['m01'] / M3['m00']) + 2*roi_height//3
                cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)

            err = cx - width / 2

            # PID控制器计算
            self.integral += err
            derivative = err - self.prev_error
            self.twist.linear.x = 0.3   
            if abs(err) > 250:
                self.kp = 1.2 
                self.kd = 0.9
            else:
                self.kp = 0.95 
                self.kd = 0.8
            self.twist.angular.z = -(self.kp * err + self.ki * self.integral + self.kd * derivative) / 600
            self.prev_error = err

            if not self.external_control:
                self.cmd_vel_pub.publish(self.twist)

    def obstacle_callback(self, msg):
        if self.is_avoiding_obstacle:
            return

        obstacle_detected = False
        for target in msg.targets:
            confidence = target.rois[0].confidence
            y_offset = target.rois[0].rect.y_offset
            height = target.rois[0].rect.height

            self.get_logger().info(f"检测到目标: 置信度={confidence}, y轴底部位置={y_offset + height}")
            if confidence >= self.confidence_threshold and y_offset + height >= self.y_axis_threshold:
                self.get_logger().info(f"符合条件的障碍物: 置信度={confidence}, y轴底部位置={y_offset + height}")
                if target.rois[0].type == "hindrance":
                    self.get_logger().info("调用避障逻辑")
                    self.avoid_obstacle(target)
                    return

        if not msg.targets or not obstacle_detected:
            self.get_logger().info("未检测到目标")

    def avoid_obstacle(self, target):
        self.get_logger().info("执行避障逻辑")
        self.obstacle_center_x = target.rois[0].rect.x_offset + target.rois[0].rect.width / 2
        self.get_logger().info(f"障碍物中心点={self.obstacle_center_x}")
        if 115 <= self.obstacle_center_x <= 440:
            self.is_avoiding_obstacle = True
            self.need_speed_reduction = True  # 需要避障时启用减速
            if 115 < self.obstacle_center_x < 180:  # 障碍在左边线边缘，需要向右进行小转角避开
                self.obstacle_direction = 'right'
                self.turn_angle('right', 0.31, 0.4)
                self.set_avoidance_times(0.4, 0.5)
                self.get_logger().info("！！向右小转角避开障碍物！！")     
            elif 180 < self.obstacle_center_x < 230:  # 障碍在左边压线，需要向右进行中转角避开
                self.obstacle_direction = 'right'
                self.turn_angle('right', 0.58, 0.6) 
                self.set_avoidance_times(0.4, 1.0)
                self.get_logger().info("！！向右中转角避开障碍物！！") 
            elif 230 < self.obstacle_center_x < 270:  # 障碍在中间偏左，需要向右进行大转角避开
                self.obstacle_direction = 'right'
                self.turn_angle('right', 0.6, 0.7)
                self.set_avoidance_times(0.7, 1.6)
                self.get_logger().info("！！向右大转角避开障碍物！！")     
            elif 270 < self.obstacle_center_x < 325:  # 障碍在中间偏右，需要向左进行大转角避开
                self.obstacle_direction = 'left'
                self.turn_angle('left', 0.8, 0.8)
                self.set_avoidance_times(0.5, 0.8)
                self.get_logger().info("！！向左大转角避开障碍物！！")    
            elif 325 < self.obstacle_center_x < 380:  # 障碍在右边压线，需要向左进行中转角避开
                self.obstacle_direction = 'left'
                self.turn_angle('left', 0.58, 0.6)
                self.set_avoidance_times(0.4, 1.0)
                self.get_logger().info("！！向左中转角避开障碍物！！")  
            elif 380 < self.obstacle_center_x < 440:  # 障碍在右边线边缘，需要向左进行小转角避开
                self.obstacle_direction = 'left'
                self.turn_angle('left', 0.4, 0.4)
                self.set_avoidance_times(0.4, 0.5)
                self.get_logger().info("！！向左小转角避开障碍物！！")
        else:
            self.get_logger().info("障碍物在边缘，继续巡线")
            self.is_avoiding_obstacle = False
            self.is_following_line = True
            self.need_speed_reduction = False  # 不需要避障时禁用减速
            self.resume_line_following()

    def set_avoidance_times(self, straight_time, resume_time):
        self.straight_time = straight_time
        self.resume_time = resume_time

    # 转弯
    def turn_angle(self, direction, ratio, turn_time):
        twist_msg = Twist()
        twist_msg.linear.x = 0.3
        self.avoid_angular_z = ratio if direction == 'left' else -ratio
        twist_msg.angular.z = self.avoid_angular_z
        self.cmd_vel_pub.publish(twist_msg)
        
        self.avoid_turn_end_time = time() + turn_time

    # 转弯后直行
    def drive_straight(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.3
        twist_msg.angular.z = 0.0
        self.cmd_vel_pub.publish(twist_msg)
        self.get_logger().info("正在转后直行")
        
        self.drive_straight_end_time = time() + self.straight_time

    # 恢复方向（略微调整方向）
    def resume_direction(self):
        twist_msg = Twist()
        twist_msg.linear.x = 0.3
        twist_msg.angular.z = -self.avoid_angular_z  
        self.cmd_vel_pub.publish(twist_msg)
        self.get_logger().info(f"略微调整方向, 角速度: {twist_msg.angular.z}, 恢复时间: {self.resume_time}")
    
        self.resume_direction_end_time = time() + self.resume_time

    def update(self):
        current_time = time()
        if self.is_avoiding_obstacle:
            if self.avoid_turn_end_time and current_time < self.avoid_turn_end_time:
                self.get_logger().info("正在转弯避障")
                pass
            elif self.avoid_turn_end_time and current_time >= self.avoid_turn_end_time and self.drive_straight_end_time is None:
                self.get_logger().info("转弯完成，准备直行")
                self.drive_straight()
            elif self.drive_straight_end_time and current_time >= self.drive_straight_end_time and self.resume_direction_end_time is None:
                self.get_logger().info("直行完成，准备恢复方向")
                self.resume_direction()
            elif self.resume_direction_end_time and current_time >= self.resume_direction_end_time:
                self.get_logger().info("略微调整方向完成，准备恢复巡线")
                self.resume_line_following()
        else:
            if self.is_following_line:
                # 确保继续巡线逻辑
                self.cmd_vel_pub.publish(self.twist)
                self.get_logger().info("继续巡线")

    def resume_line_following(self):        
        self.is_following_line = True
        self.is_avoiding_obstacle = False
        self.avoid_start_time = None
        self.avoid_turn_end_time = None
        self.drive_straight_end_time = None
        self.resume_direction_end_time = None
        self.prev_error = 0.0
        self.integral = 0.0

        if self.need_speed_reduction:  # 判断是否需要减速
            # 速度和转向角度的渐变初始化
            initial_linear_speed = 0.06  # 初始线速度
            target_linear_speed = 0.3  # 目标线速度
            steps = 5  # 恢复速度的步数
            self.get_logger().info("步进恢复巡线操作")
            
            for i in range(steps):
                # 计算当前步的线速度
                self.twist.linear.x = initial_linear_speed + (target_linear_speed - initial_linear_speed) * (i + 1) / steps
                
                # 使用当前的巡线误差来计算角速度
                err = self.prev_error
                self.twist.angular.z = -(self.kp * err + self.ki * self.integral + self.kd * (err - self.prev_error))
                
                # 发布控制指令
                self.cmd_vel_pub.publish(self.twist)
                
                # 等待一小段时间
                sleep(0.09)
        else:
            self.twist.linear.x = 0.3  # 直接恢复到正常速度
            self.cmd_vel_pub.publish(self.twist)
            self.get_logger().info("不需要避障，不减速恢复巡线")

        self.get_logger().info("恢复巡线状态设置完成")

def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
